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ABSTRACT 


A sequential Extended Kalman Filter and Smoothing 
routine was developed to provide real time estimates of 
torpedo position and depth on the three dimensional under- 
Water tracking range at the Naval Torpedo Station, Keyport, 
Washington. Inputs to the routine were acoustic pulse 
transit times from the target to receiving array elements 
whieh are non-linear functions of the position coordinates. 
These inputs were linearized and the filter gains and 
filtered estimates calculated on-line. By using a smoothing 
Subroutine, all past filtered estimates were smoothed. 
Tests were conducted using simulated torpedo trajectorie 
that traversed multiple hydrophone arrays. It was found 
that filter performance was dependent on system noise and 
the distance the torpedo was from the hydrophone array and 
the smoothed estimates of states were better than or equal 


to the filtered estimates. 
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I. INTRODUCTION 


Washington currently operates two 


(3-D) underwater 


tracking range utilizing 
to be tracked. 


LS@synehnronized with a master clock. Timed 


are received by bottom mounted hydrophone 


relayed via cable to a computer at the 


The computer calculates the positional 


hr 


trajectory through 


which consists of the elapsed time 


FS AW de | 
ts recel tC the 


ot 
1s corrupted with noise due 
of 


and measurement 


ar 


«Db 


and measurements 


on 


in order 
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a) 
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Exists for expanding the capability ef 


time ‘Kalman’ Filter and post 


widen can take as ani input 


pulses, and produce the be 


esoimace Of the position of the tracked object at a 
particular time. Previous research in this area [3] and 
[4], revealed that a Kalman filter utilizing a sequential 
estimation approach was desirable. 

The intention is to develop and test a sequential Kalman 


filter and smoothing algorithm that can be interfaced with 


the current underwater range system. 


TE. DESCRIPTION OF RANGE TRACKING GEOMETRY 


The hydrophone array, consisting of four independent 
elements, defines an orthogonal coordinate system in which 
transit time measurements are made. As shown in Figure 1, 
mour hydrophones X, Y, Z, and C are on four adjacent 
vertices separated by a distance d, along the edge of the 
Cube. The origin of the array coordinates is at the center 
of the cube with the orthogonal coordinates parallel to its 
edge. Positional information is computed from the transit 
times of a periodic synchronous acoustic signal travelling 
from the torpedo to the four hydrophones on the array. T 
range measures the tracked torpedo's position every 1.31 
Beconds to an accuracy that is typically within 3 to 30 
feet. A more detailed description of the range tracking 


Capability is described in [2]. 


“y bone Eran 


a 
e* 7 


‘co te ae y 


co ¢ i 


S@fS 


: T. 
ae 
HYDROPHONE Z Se pees 


Cad /2.—d/ 25 di/ 2) 
4 


HYDRUPHONE C 
(<d/2,-d/2,-d/2) 


| 
| 
| 
HYOROPHONE X J 
(d/2,-d/2 
-d/2) 


Figure 1. 


HYDROPHONE Y 
( -d Hf 2 > d if 2 , = / 


Geometry of a Tracking Array 


LE 


2) 


“/ 


if 


A. THE EXTENDED KALMAN 
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Since the transit times were readily available and are 


nonlinear functions of position, these equations can be 
linearized and Kalman filter theory applied using the 
extended Kalman filter. This procedure produces a real-time 
System, filtering on the transit times T,, Mee, Ty and 
without the necessity of converting these times to 
positions. 
For the three-dimensional location problem thre 

position states (x, y, z) and two velocity states (v,, vy? 
Specify target motion. tIhe discrete linear and nonlinear 
observation equations are given by 

my fad a Po (ke) + wl k) Carle) 
and 

Eee) = Weak), ke) + vk) C2) 
Ma chese equations ® and ' are constant matrices and h is a 
MOnlinear function of the state variable Re WK) 2s) plant 
excitation noise and v(k) is measurement noise. The plant 
noise and measurement noise are assumed uncorrelated (white) 
Meum zero mean. That is, 


alee 


mc = wea) d= Q'(k) 6,3 


J 
and 
Etv(k) * vi (3)] = R(k) 65 
with 
Si alii Fe a Sale 5 


inorder to apply -the’linear filter equation (3.2) is 
expanded in a Taylor series about the best estimate of the 
Suate at that time and only tne first-order terms are kept. 


Bo@uation (3.2) gives 


AG) = H(k) * x(k) + ¥(k) C3530) 
wnere 
H(k) 2 22 (3.3a) 
Ba nets Gina Cky, R=) 


¥(K/k-1) is a predicted value of the state before the kth 
measurement. 
A state error vector is defined by 


Ge ise (K7K) = x(k), 
and a predicted state error vector is defined by 
BAG] Xk ( K/h a1) = 1x (Kk) « 
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The covariance of state error matrix is defined by 
Ptky/ ke) “s BLYCK/E) at 9 OG Ae as ae 


and the predicted covariance of state error matrix is given 


by 


P(k/k-1) = ELR(K/k=1) * % (k/k=1)] 


The state e@xeitation matrix is given by 
j } oa ey Ny T } 
Deo air ie) eiwik) Sw Ck) « ro (kK) 
and the measurement noise covariance matrix is 


R(k) = Elv(k) * vi (k)] 


The Kalman filter equations are given by [1]: 
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P(k+1/k) = @P(kK/k)o~ + Q(k) (3.4a) 
G(k) = P(k/k-1)H!(«)CH(k)*P(K/K-1)H Ck) + ROCKIT! (3.40) 
Bake PhyehGCk) He) ) | PLK/ ke (3.4e) 
Ree /K) = © x(k/k) (3.4d) 
BG) Kei) = h(x k/k=1),. k) (3.4e) 
K(k) = X(k/k-1) + G(k)[z(k) = 2(k/k-1)] (3.4f) 


The Q matrix serves. not only to allow for maneuvering 
put alse to account for any model inaccuracies, that is, any 
discrepancies between the true action of the torpedo and its 
emeraccerization py Equation (3.1). The Q also serves to 
prevent the gain matrix G(k) from approaching zero by always 
insuring uncertainty in the predicted covariance of error 


Matrix P(k+1/k). 


B. OPTIMAL LINEAR SMOOTHING 

Smoothing is a non-real-time data processing scneme tnat 
uses all measurements between 0 and T to estimate the state 
waa System at certain time t, where 0 < t-< T. The 
Smoothed estimate of x(t) based on all the measurements 
between 0 and T is denoted by X%(t/T). 

Smoother error covariance is denoted by P(t/T) and 
exe/1) < P(t) means that the smoothed estimate of x(t) is 
always better than or equal to its filtered estimate. This 
iS shown graphically in Figure 2. 

several forms of the smoothing equations may be derived. 
One is the Rauch-Tung-Striebel form, which was used in our 


particular case with the discrete-time expressions 


4) 


Summarized as follows [1]: 


RCK/N) = &(k/k) + ALERCK+1/N) - &(k+1/k)] (3.5a) 
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Figure 2. Advantage of Performing Optimal Smootning | 


where 
A Apne Olt? #8 covawi4 malin | 
a, = POs/k)@(k) PCk+1/k) ror kK = N-1 
P(K/N) = P(k/k) + AL [P(k+1/N) = P(k+1/k) IA,” 
meso for k = N=1. 
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In tnese equations xX(K/N) is smoothed State Estim 


foresN) is Error Covariance Matrix Propagation. 
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mio” PROBLEM DEFINITION = TORPEDO TRACKING WITH THE EXTENDED 
KALMAN FILTER AND OPTIMAL SMOOTI 


wee FLUTTER EQUATIONS 
In the torpedo tracking problem, the non-linear 
observations are the four independent transit times from the 


tracked object to the hydrophones, De oe, Tendat... lus 


ke 
t 


the non-linear measurement matrix Z{%) is defined as: 


T,(k) Ter l x(k) +d/2) “+ (yk) 44/2) °*(2(k) 44/2) 917 Pav (k) 
T.(k)] | ppp (x(k) = 12) 24 Cy Ck) 14/2) 24 (2) 44/2) 211/24 Ce) 
= | 

z(k)= = 
z | | 
Gey ed 2) ay (ed 72-4 (2k) 4d/2)7 1)” 40k) 
y VEL 

2) DeK | 

Tk) Parl (x(k) #d/2)°+ (y Ck) 44/2) o + ( Bk ease) 1 ee) 


The meaSurement noises, v(K)'s, are assumed to be zero-mean 
and independent with a covariance matrix 


—- —— 


os 0 0 0 
0 38 0 0 
R(k) = x Gi) 
0 0 5 0 
y 


Pauacton (3.34) can be used to give the linearized 
observation matrix. When the derivatives are taken and 
@€valuated at the predicted state values x(k/k-1) = x'(k) the 


result is 


x'(k)+d/2 0 y'(k)+d/2 0 Z1(k)4d7 2 
ef a =o Se ia em 
x'(k)-d/2 0 y'(k)+d/2 ; ZC eae 
, | DEN2— = Dro =o eee 
pkieas2  , vilk)-a/2 9 2'(k)+a/2 
sao). aa aio eae = pence 
x'(k)+d/2 0 y'(k)+d/2 0 Zi CkjadZe 
7 DrNuT Sey a = eN 
where 
Pe : P) 
DEM l= T(x" (Ck) 4d/2)°+ Cy (CR) 40/2) “4 (2? Ck) 4d/2)°1 1/2 
oe 2) 
Deere) ay 2-207) (esd 2) 2 (kK) ease)" 1’ * 
: ee. 42 | D4 2 
BEN 3 = P(x" (k) 4d/2)+(y' (k) -4/2) needy 
2) eral es 
Bey 2) cet (e) 1d/29 2 Gy (Kk) 4d/2) 7202! (ke) =6/2)"1'"* 


The torpedo dynamics used for the tracking problem are 


4 a 


assumed to be 1/S” with estimations on five states x 
Besizi0on, x velocity, y position, y velocity and z position 
(height of torpedo above hydrophone array). 


ame means of the random excitation and random noise are 


seoumed to be zero, 1.¢., 
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i 
(2) 


ELw(k)] 


ELv(k)] 


i" 
(@) 


Four measurements are taken every 1.31 seconds, which is 
‘ 3 P : hoe : i / 2] 
Ome time slot, and with this sampling time the 1/S” plant 


has state transition, (PHI) and gamma, ('T) matrices equal 


Aor 
7 | 
1 i 0 0 0 | 
0 1 0 0 0 
a= 0 0 1 T 0 (4.4) 
0 0 0 1 0 
0 0 0 0 1 
and 
1/2 0 0 | 
7 9 0 | 
r= 0 pve 0 | (4.5) 
0 i 0 | 
0 0 i | 


eat mavrix, O Matrix, Rh matrix, and H matrix are then 


used in the Kalman filter equations (3.4). 


a". i ae 


ee £9064 


oe O08 Sete. e 
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B. THE SEQUENTIAL EXTENDED KALMAN FILTER 

In the sequential approach, the basic Kalman filter 
equations (3.4) must be modified. Calculations are 
performed on each of the four independent transit times in 
mae following order: T_, es TY and ue for’ each 1.31 second 
Gime slot. The estimate of the states x(k/k), based on one 
transit time measurement are used as the prediction x(k/k-1) 
for the calculations on the next measurement. Thus for the 
first time measurement te enly rine fairst row of the 
Hinearizing H matrix is calculated. 

Hex: the first “gain column corresponding to the first 


time measurement ae is calculated by 


ek 
P(k/k-1) one 


; = Ch. &) 
i1eo1 S ’ ae 
Moe eee ee + Ros 


m@ere' i = 1 to 4 corresponding to tne four measured transit 
Games. lnus, the first row of the H matrix is used to 
Calculate the first column of the gain matrix with both 
eorresponding to the first measured time 

Next, an estimate of the particular observation time is 
Sateulated using equation (3.4f) evaluated at the predicted 
State x(k/k-1). 

The difference between observed transit time and tne 
eouumaved transit times forms the residual whicn is used in 


the estimate equation 
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x. 5 x(eK=ST)> +:'G. 


ico, Residual] ie 


This equation gives an estimate of the states based on one 
of the four time measurements. 
Next, covariance of error is calculated based on one 


measurement by 


Paweo [de =); oh P. Ne 3 
qi [ icol ow 1-1 ( 
where 

if seidentity matrix 

P = the covariance matrix calculated from the 


previous transit time measurement or if i = 1, 
the prediction P(k/k-1). 
meer the first iteration, x, becomes x(k/k-1) and Py 
becomes P(k/k-1) for the second iteration which calculates 


the estimate of the states based on the second measurement 


Bs 
i 
Aieer four iterations (k = 4), ky becomes the estimate 
mer Ghe time slot x(kK/k) and P, becomes the covariance error 
Eak/k). 
Hae wpredictions fer the next time slot are calculated 


Meamg equations (3.4a) and (3.4d). This process is repeated 


for each time slot. 
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eer GPLTIMAL SMOOTHING, PROCESS 

During the running of the Extended Kalman filter and 
Smoothing routine, after the forward filter pass for each 
time slot (except the first), the smoothing subroutine is 
called. By using the present and previous filtered estimate 
@ox(t), a smoothed estimate of previous x(t) is calculated. 
his process is repeated for each past time slot. 

solution of the equations (3.5) proceeds as follows: As 
an example, and because it is slightly easier to see when 
actual times are used, suppose N = 30. On the forward 

/k-1), P(k/k), and 


P(k/k-1) would be computed and stored. On the final 


x 
iT) 
= 
tl 
Lo 
co) 


meerdation of the-ferward pass, with 
pose / 30) = x (30729) + G(30) [2(30) - H x(30729))] 


i.e., we have computed and stored (30/30). 


Now, the smoothing process starts in the reverse 


Garection. Decrement k to k =N = 1 = 29, then 
£(29/30) = (29/29) + A(29) [&(30/30) - &(30/29)] 
stored stored stored 
Tk a 3 = 
and AC29) = B29 7 29) 2 Pl307 29°) 
stored stored 


‘= 92 gu ilen, L~ 
—_ ? 


tn *eM2oqme VES 


"oiszekg 


.sla 


bet k = N «= 2 


&( 28/30) 


and A(28) 


Also, for each 


P( 29/30) 


P(28/30) = 


28, then 


(28728) 532428)" 1x(29/30)\ = ¥(29/2 


stored computed 
Last 


iteration 


P(28/28) 2) p(29/28)7! 
stored stored 


of the two preceding iterations, 


P(29/29) + A(29){P(30/30) - P(30/29)] A (29) 


ee ee 
wee SE 


stored computed stored 


P(28/28) + A(28){P(29/30) - P(29/28) 


i enntiertilteenall 
wwe EEE tee 


stored computed stored 
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V. TESTING AND SIMULATION 


Mae DESCRIPTION 

The sequential Extended Kalman Filter and Smoothing 
routine is tested using simulated torpedo tracks. A variety 
of track scenarios were produced to test the filter and 
smoothing performance during single and multiple arrays 
tracking. 


Computer generated tracks were tested in the first 


b- 


series of straight running, constant depth and constant 
velocity torpedoes. A variety of track scenarios were used 
Bransiting through multiple quadrants including: 

ion GCressing «northe of »the array. 

Pooucrossingsdiagonaliy through the array. 

The next series of tests demonstrates the ability of the 
mercer to track through the areas of multiple arrays 
maciluding: 

1. Crossing above the arrays. 

e.e Grossing diagonally through the arrays. 

All runs were made with a variety of initialization 
€rrors in position and velocity. 

Zero mean Gaussian noise is added to corrupt the 


eeserved transit. times for all runs. 
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B. THE GATING SCHEME 

The operation of the filter may be adversely affected by 
large measurement noise. One error of a relatively large 
magnitude could invalidate the filtered output for many 
subsequent time slots. Before random measurement noise and 
random excitations could be added to tne observed times for 
testing, a form of protection was designed to guard against 
Gatastrophic failure. This protection is provided by 
Ssuablishing limits of acceptability for each of the 
measurements. 

Measurement errors can occur because of many factors 
Mieluding an ierror in the transit. time of the acoustic pulse 
Primarily due to the receipt of multipath signals that have 
Bounced off the surface, bottom or different density layers. 

A three-sigma gate was designed using the covariance of 
measurement noise (R) and the covariance of estimation error 
me ok/k)). 

For each calculation of a state estimate (x(k/k)), the 
largest positional covariance of error was used, either x, y 


Or z, and converted to time in seconds using the average 


Mevoecity of sound in water for Dabob Bay, 4860 ft/sec. The 
maee then was written for each time measurement i = 1 to 4: 


PCY Kane 
GATE = Lemeese 


ee Ne, | ij 
(4860. ) oh 


PO 
ie i] 


The gate expands or decreases depending on tne confidence 
Mevyel of the position estimate and the transit time. If 
ZDIFF, which is the difference between the actual transit 
time received and the predicted transit time to a particular 
hydropnone, exceeds the gate, the measurement is considered 
unacceptable and the filter gain is set to zero causing the 
filter to ignore the data and take the prediction of the 


states as the estimate 
x(kK/k) = x(k/k-1) 


An invalid time measurement zeros only the gain column for 
mmat particular hydrophone causing only that hydrophone's 


data to be ignored. 


me MULTIPLE ARRAY TRACKING 

initial tests were performed on-tracks..in the..area of 
One array. In order to more closely simulate a typical run 
On the range, a scheme was designed to track the torpedo 
BArough multiple arrays. 

First, a coordinate system is defined as shown in 
Meeure 3. The center of the coordinate system is geographi- 
Cally near the entrance to Dabob Bay in the simulation. 
meray number 6 is the closest array to be coordinate center. 
ieecthe “simulation array 1 is at 36,000 feet from coordinate 
Geucrer and array 6 is 6,000 feet. The C hydrophone is 


assumed to be the axis location of each array. Then each X 
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position for the X hydrophone in each array is ce + 30, each 
Y position for the Y nydrophone is Y, + BO, and: ache Z 
position for the Z hydrophone is Z, + 30. These 72 
positions, an XYZ poSition for each of 4 hydrophones in 6 
Brrays, were placed into a 6 x 12 matrix HYDRO and 
referenced throughout the routine. 

The geometry centered on each array is taken out of the 
problem and the target position is based on a central 
reference. 


The non-linear time equation becomes 


2 2 2 
Ls EO Ae - ey + (y - : eg. tae: ) 


where x Okez. Ls) the pesition of a particular hydro- 


9’ Ta fe) I 
phone and array being used. 

The decision parameter used to determine the switching 
meen array to array is a straight handoff. If the predicted 
Bepesition is greater than 3,000 feet from the array in use, 
then an index (18) is incremented and the next row of HYDRO 


is implemented. This placed into the routine the x, y and z 


Mm the next array: The handoff 


h- 


positions of the hydrophones 
Can easily be utilized in real range operations, as tne 
transit times from adjacent arrays are present at the 
Sempucer for a particular time slot. For simulation, it is 
assumed that in all the arrays each axis pointed in the same 


Eeeection. In actual range operations each array is tilted 
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apes boch the X and Y axis. Since the true transit times 
are derived in a tilted coordinate frame, the filter's 
Esauimace Of transit. time must also be calculated in a tilted 
coordinate frame. The tilt angle measurements along with 
the level rectangular coordinates of the array with respect 
merchne central rectangular’ coordinate system can be input 
into the matrix HYDRO to rotate the coordinates of each 


hydrophone in the array. 


Vib (he ol RESULTS 


ae  SERLES .ONE 

Figure 5 shows the true trajectory of the torpedo in the 
Morazontal X-Y plane during a straight run through single 
eeray. Torpedo velocity is 50 knots in the x-direction. 
Migtial position errors are set to 25 feet for X and Y. 
Velocity errors are set to zero. Figures 6, 7 and 8 depict 
the position errors for both Kalman filter and Smoothing. 
Measurement noise is added to all runs. The steady state X 
and Y position errors ranged between -6 and +9 feet through- 
out the trajectory for Kalman filter and -2 and +4 feet for 
Smoothing. The position errors are computed by subtracting 
mie tilcer position estimate, x(k/k), for Kalman filter and 
x(k/N), for smoothing, from the computer 
mas2on for each time slot. Figures 9, 10, 11, ltée, and 13 
depict the mean square estimation errors of states. These 
estimation errors are obtained by taking the appropriate 
diagonal terms of the covariance matrix and smoothed 


Covariance matrix. 


Ber SERIES TWO 
Figure 14.shows the true trajectory of the torpedo in 
Dne horizontal X-Y plane, during a crossing run through 


Samgle array. Torpedo velocity is 40 knots in X=direction 
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1 


aad’ 25 knots’ in Y=direction. The torpedo depth is 
Maintained at 300 feet. Figures 15, 16, and 17 depict the 
Positaon ‘errors. Since’ the initial position errors are set 
to zero, the position errors ranged between -3 and +4 feet. 
fezares 16, 197, 20, 21, and 22 depict the mean square 


Betimacvion errors: of states. 


tee SURIES THREE 

Pigure 23 shows the true trajectory of the torpedo in 
the horizontal X-Y plane, during a straight run through 
multiple array. Because of storage problem of the computer, 
the runs through the multiple array were made for 190 time 
mers. torpedo velocity is 50 knots in X=direction and the 
depth is 100 feet. Figures 24, 25, and 26 show the position 
errors ranged between -6 and 17 feet. Figures 27, 28, 29, 
gos and 31 depict the mean Square estimation errors of 
Beaces. Figure 32 shows the error ellipsoids superimposed 
at every eighteenth observation. The error ellipsoids are 
expanded to twenty-five times their true value in order that 
they may be seen. The error ellipsoids provide a geometric 
interpretation of the behavior of tne estimator. Before tne 
Wand-off point, at the ninetyth time slot, the major axis 
Boeaclon of the error ellipsoid and magnitude of the axi 
were -16.98 degrees and 43.28 feet, respectively. After the 
hand-off point major axis rotation became 25.8 degrees, and 


1ts magnitude became 1.6 feet. When the magnitude of an 


axis of the ellipsoid decreases, the conclusion is that the 
error in the estimate decreases, because the observation 
from the new array has an error covariance ellipse rotated 
over 40 degrees from the present covariance (-16.94 degree) 
of the estimate. The ellipsoids are extremely narrow. When 
combined the resultant covariance is reduced greatly. 

Preure 4 depicts the result of reduction and rotation of the 
@llipsoids. Figure 33 shows the error ellipsoids before and 


after the hand-off point. 


. 


Figure 4, Result of Rotation and Reduction of the 
Error ELlapscoids 


De SERIES FOUR 


Pugiress. svows tae true trajectory of the torpedo in 


mae Horizontal X-Y plane, during a crossing run 
MiicipLle array. Torpedo velocity. is’ 50 knots in 
and 40 knots in Y-direction. The torpedo depth 
tained at 300 feet. imitidale posa tion. errors, are 
meet for X and Y and adnitial velocity errors are 
Paets. Figures 35, 26, and 37 show the position 


errors. Since the initial position and velocity 


through 


X-direction 


is main- 


sete bo: 25 


= 


Siete tier .5 


set to 25 feet and 5 knots, the big position errors were 


taken at the beginning of the run. These values 
ignored from the figures in order to see clearly 


Seeec ne. run... Figures 38, 39, 40, 41, and 42 show 


were 


Square estimation errors of states for both filtered and 


Smootned. 
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Vil. CONCLUSIONS 


The sequential Extended Kalman Filter and Smoothing 
satisfactorily provided real time estimates of torpedo 
position and depth. The average of steady state position 
and depth errors ranged between 3 and 1 feet for torpedo 
tracks within the specified radial tracking range after 
Kaiman filter. These errors had a range of around 1 foot 
after smoothing. 

The filter performance was dependent on system noise and 
the distance the torpedo was from tne hydrophone array and 
the smoothed estimates of states were petter than or equal 
to the filter estimates. 

Implementation at the range computer facilities can be 
accomplished by real time Kalman Filterir 
smoothing of the raw time data. Future tests should include 
evaluating filter performance using trajectories generated 
from actual torpedo runs on the Dabob test range jh 
tests would verify the adequacy of the noise model in 
Filter and the ability of the software to edit erroneous 
transit time measurements. 

the rotation and reduction of the error ellipsoids 
(i.e., the filter error covariance) was most instructive and 


fawe much insight into the performance of the filter. 
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Figure 34. 
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APPENDIX A 


PROGRAM DESCRIPTION AND FEATURES 


The sequential Extended Kalman Filter and Smoothing 
routine used for torpedo tracking is modularized for ease of 
implementation. The program is general in nature and many 
Of the parameters of the filter are variable including: 

a. The number of states in the filter -- N 

b. The number of random forcing functions -- M 

ec. The number of measurements -- JS 

d. Number of time slots -- JTIME 
The constant matrices PHI, R, COVW, and GAMMA are 
initialized in the beginning of the program using data 
aeabements. The fiiter is initialized with P(1/0) and 
meiyO) (initial covariance of estimation error and states) 
Beane subroutine INIT. The first estimate is at time 1 and 
eontinues until ITIME = JTIME + 1. True measurement times 
(ZI) are computed using either subroutines TRAJEC or TRAJC3, 
depending on whether single array or multiple array tracking 
is implemented. Either subroutine will compute four 


measurement times (T_, 


A te le) for each time slot. The 


ae 

Reve way 
measurement times are corrupted by zero-mean, white Gaussian 
noise using the IBM-3033 subroutine GGNML. For each of the 


four time measurements the corresponding row of the 


Vv * ee = eee 
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linearizing H matrix is calculated using either subroutine 
CHROW or CHROW3, depending on whether single array or 
multiple array tracking is used. The corresponding gain 
matrix column GI is then found. These row and column values 
are utilized in forming the covariance of estimation error. 
PI, for that particular time measurement. Next the estimate 
of the observation time ZHAT from that particular hydrophone 
ma formed Wsing..the subroutine CZHAT or CZHAT3, depending on 
whether single or multiple array tracking is implemented. 
tie residual ZDIFF (GL) = ZIC(@L).=.ZHAT, is then calculated. 
Fimally the estimate of the states XI based on one time 
measurement is calculated and the process is repeated for 
the next measurement. After four iterations, XI becomes the 
State estimate XKK and PI becomes the updated covariance of 
estimation error PKK, and the predictions of the states and 
COVariances XKKM1 and PKKM1 are formed. Finally, for each 
time slot (except the first) smoothed state estimates, XKKS, 
and covariances, PKKS, are formed using the subroutine 
SMOOTH. PLOTP is used to generate line printer plots and 


PLOTG is used to generate VERSATEC plots. 


fey PROGRAM SUBROUTINES 

A brief description of the subroutines are described 
below: 

1. TRAJEC -- This subroutine develops the torpedo 


erajectory which is used as truth data for the filter. The 


Ti 
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BuOrouLuiMe OQULpULS true transit times, ZI(1), and the x, y, 
Mmeuesthrons, ©DCr), or the torpedo for each time slot. THe 
Subroutine is used during single array tracking. 

2. TRAJC3 -- This subroutine performs the same function 
Sen inAJEC but is used only during multiple array tracking. 

3. INIT -= This subroutine generates the initial state 
wectcor x(0/=1) and initial covarianee matrix P(0/-=1). 

4, CHROW -- This subroutine computes the appropriate 
how @f the linearizing H matrix. Each row corresponds to 
one of the four transit time measurements, ee a Ty ee 
fais Subroutine is used during single array tracking. 

>. CHROW3 =— This subroutine performs the same 
functions as CHROW but is used only during multiple array 


wracking. 


m 
ct 
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6. CZHAT -- This subroutine computes the estim 
meansic times for the filter. Four transit times, ZHA) 
Calculated corresponding to each of the four true transit 
memes Z21(t). This subroutine is used during single arrai 
tracking. 

7. CZHAT3 -= Subroutine performs same functions as 
CZHAT however it is used only during multiple array 
tracking. 

8. QFIND -= This subroutine develops an adaptive Q 
Matrix which is a funetion of the torpedo velocity. Three 
input variables defined in a data statement at the beginning 


of the program can be adjusted: 
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a. ae 
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aa. SIGACC -- Maximum expected horizontal 
acceleration of the torpedo. 

bb. SIGDIV -- Maximum expected cnange in vertical 
velocity. 

ec. SIGCC -— Maximum expected turn rate of the 
merpedo in the horizontal. plane. 

The values listed in the program were used and kept 
monscant during the simulation tests. If the user desires 
not to use the adaptive Q subroutine, software code is 
provided at the beginning of the program to calculate a 
constant Q matrix. 

9. GGNML -= This is an IBM-3033 subroutine contained in 
the IMSL library. The routine generates zero mean white 
Gaussian noise with an RMS value normalized to 1. The main 
program scales the noise and adds it to the transit time 
measurements. 


WO % PLOTP == This is an IBM-3033 subroutine used to 
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generate the line printer plots. nformation on this 
Subroutine can be obtained from the IMSL library. 

Meer wOnG m= chas iS «an [BM=3033, subroutine used to 
Benmerate the VERSATEC plots. information on this subroutine 
can be obtained from the IMSL library. 

12. SMOOTH -- This subroutine computes the smoothed 


State estimates and covariances. 
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Be UTILITY PROGRAMS 


These subroutines were designed to be used for 


repetitive matrix and vector manipulations: 


‘Wye 


SON, Ot 


PROD -- multiplying two matrices 

MMULT -= multiplying a matrix and a vector 
VMULT -=- multiplying two vectors 

TRANS -- transposing a matrix 

ADD -- adding two matrices 

SUB == subtracting two matrices 


RECIP -- inversing a matrix 
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SEQUENTIAL EXTENDED KALMAN FILTER AND 


SMOOTHING PROGRAM LISTING 
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